#include <MeetAndroid.h>

/***********************
 Bluetooth test program
***********************/

MeetAndroid meetAndroid;

unsigned long currentMillis = 0;
long previousMillisCircle = 0;
long previousMillisBlinking = 0;
long previousMillisVibrator = 0;

const int pinVibratorAlert = 2;
const int pinBlinkingLedLeft = 3;
const int pinBlinkingLedRight = 4;
const int pinCircleLedA =  5;
const int pinCircleLedB =  6;
const int pinCircleLedC =  7;
const int pinCircleLedD =  8;
const int pinCircleLedE =  9;

int statesActual[] = {LOW, LOW, LOW, LOW, LOW};
int stateBlinking = LOW;
int stateVibrator = LOW;
boolean vibratorOn = false;
boolean ledCircleOn = false;
int ledMs = 0;

void setup() {
  Serial.begin(57600);
  meetAndroid.registerFunction(vibratorStart, 'w'); // w=warnung
  meetAndroid.registerFunction(vibratorStop, 's'); // s=stop
  meetAndroid.registerFunction(velocityChange, 'v');
  pinMode(pinVibratorAlert, OUTPUT);
  pinMode(pinBlinkingLedLeft, OUTPUT);
  pinMode(pinBlinkingLedRight, OUTPUT);
  pinMode(pinCircleLedA, OUTPUT);
  pinMode(pinCircleLedB, OUTPUT);
  pinMode(pinCircleLedC, OUTPUT);
  pinMode(pinCircleLedD, OUTPUT);   
  pinMode(pinCircleLedE, OUTPUT);   
}

void loop() {
  // Save current time at every loop
  currentMillis = millis();

  // Testmode to test without meetAndroid-Events
  //ledCircle(1000);
  //ledBlink(1000);
  //blinkingLight('b', 500);
  //vibratorAlert(1000);

  meetAndroid.receive(); // wait for Android commands
  
  // LED-Control
  if (ledCircleOn == true)
    ledCircle(ledMs); // Blinking LED-Circle for driving
  else
    ledBlink(1000); // Blinking LED-Circle for standing
  
  // Vibrator
  if (vibratorOn == true)
    vibratorAlert(1000); // Vibrates with an interval (on/off) of 1000ms
  else
    digitalWrite(pinVibratorAlert, LOW); // Turn vibrator off   
}

// ---------------------
//   Led Circle
// ---------------------

void velocityChange(byte flag, byte numOfValues) {
  float velocity = meetAndroid.getFloat();
  Serial.println(velocity);
  if (velocity > 0.5) {
    ledMs = velocityToMs(velocity);
    ledCircleOn = true;
  }
  else {
    ledCircleOn = false;
  }
}

int velocityToMs(float velocity) {
  int velocityKmh = velocity * 3.6;
  int ms = map(velocityKmh, 0, 50, 400, 10);
  return ms;
}

void ledCircle(int interval) {
  if (currentMillis - previousMillisCircle > interval) {
    if (statesActual[0] == HIGH) {
      statesActual[0] = LOW;
      statesActual[1] = HIGH;
    }
    else if (statesActual[1] == HIGH) {
      statesActual[1] = LOW;
      statesActual[2] = HIGH;
    }
     else if (statesActual[2] == HIGH) {
      statesActual[2] = LOW;
      statesActual[3] = HIGH;
    }
     else if (statesActual[3] == HIGH) {
      statesActual[3] = LOW;
      statesActual[4] = HIGH;
    }
    else {
      statesActual[4] = LOW;
      statesActual[0] = HIGH;
    }
    previousMillisCircle = currentMillis;
  }
  ledOnOff(statesActual);
}

void ledBlink(int interval) {
  if (currentMillis - previousMillisCircle > interval) {
    if (statesActual[0] == HIGH) {
      statesActual[0] = LOW;
      statesActual[1] = LOW;
      statesActual[2] = LOW;
      statesActual[3] = LOW;
      statesActual[4] = LOW;
    }
    else {
      statesActual[0] = HIGH;
      statesActual[1] = HIGH;
      statesActual[2] = HIGH;
      statesActual[3] = HIGH;
      statesActual[4] = HIGH;
    }
    previousMillisCircle = currentMillis;
  }
  ledOnOff(statesActual);
}

void ledOnOff(int stateArray[]) {
  digitalWrite(pinCircleLedA, stateArray[0]);
  digitalWrite(pinCircleLedB, stateArray[1]);
  digitalWrite(pinCircleLedC, stateArray[2]);
  digitalWrite(pinCircleLedD, stateArray[3]);
  digitalWrite(pinCircleLedE, stateArray[4]);
}

// ---------------------
//   Blinking Light
// ---------------------

void blinkingLight(char light, int interval) {
  if (currentMillis - previousMillisBlinking > interval) {
    if (stateBlinking == HIGH) {
      stateBlinking = LOW;
    }
    else {
      stateBlinking = HIGH;
    }
    previousMillisBlinking = currentMillis;
  }
  if (light == 'l') {
    digitalWrite(pinBlinkingLedLeft, stateBlinking);
  }
  else if (light == 'r') {
    digitalWrite(pinBlinkingLedRight, stateBlinking);
  }
  else if (light == 'b') {
    digitalWrite(pinBlinkingLedLeft, stateBlinking);
    digitalWrite(pinBlinkingLedRight, stateBlinking);
  }
}

// ---------------------
//   Vibrator Warning
// ---------------------

void vibratorStart(byte flag, byte numOfValues) {
  vibratorOn = true;
}

void vibratorStop(byte flag, byte numOfValues) {
  vibratorOn = false;
}

// Vibrator control function
void vibratorAlert(int interval) {
  // check to see if it's time to change the state of the vibrator (on/off);
  // that is if the difference between the current time and last time a state
  // change occured is bigger than the interval at which the vibrator should
  // be turned on or off.
  if (currentMillis - previousMillisVibrator > interval) {
    if (stateVibrator == HIGH) { // turn it off if it's on
      stateVibrator = LOW;
    }
    else {
      stateVibrator = HIGH; // or turn it on if it's off
    }
    // save the last time the vibrator state changed
    previousMillisVibrator = currentMillis;
  }
  // write output pin where the vibrator is attached
  digitalWrite(pinVibratorAlert, stateVibrator);
}
